home *** CD-ROM | disk | FTP | other *** search
/ Aminet 2 / Aminet AMIGA CDROM (1994)(Walnut Creek)[Feb 1994][W.O. 44790-1].iso / Aminet / util / misc / Fudgit233.lha / Source / src / mrqmin.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-12-14  |  10.1 KB  |  326 lines
  1. #include <stdio.h>
  2. #include <math.h>
  3. #ifndef NOUNISTD_H
  4. #include <unistd.h>
  5. #endif
  6. #include "dalloca.h"
  7. #include "head.h"
  8.  
  9. #define ERRR (-1)
  10.  
  11. static int mrqcof(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **alpha, double *beta, double *chisq);
  12. static int gaussj(double **a, int n, double **b, int m);
  13. static int covsrt(double **covar, int ma, int *lista, int mfit);
  14.  
  15.  
  16. int Ft_mrqmin(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **covar, double **alpha, double *chisq, double *alamda)
  17. {
  18.     int k,kk,j,ihit;
  19.     static double *da = 0;
  20.     static double *aold = 0;
  21.     static double **oneda = 0;
  22.     static double *beta = 0;
  23.     static int sma = 0;
  24.     static int smfit = 0;
  25.     static double ochisq;
  26.     double *Ft_dvector(int nl, int nh);
  27.     double **Ft_dmatrix(int nrl, int nrh, int ncl, int nch);
  28.     void Ft_free_dmatrix(double **m, int nrl, int nrh, int ncl, int nch);
  29.     void Ft_free_dvector(double *v, int nl, int nh);
  30.  
  31.     if (*alamda < 0.0) {  /* clean and allocate on start */
  32.         if (oneda)
  33.             Ft_free_dmatrix(oneda, 1, smfit, 1, 1);  oneda = 0;
  34.         if (aold)
  35.             Ft_free_dvector(aold, 1, sma); aold = 0;
  36.         if (da)
  37.             Ft_free_dvector(da, 1, sma); da = 0;
  38.         if (beta)
  39.             Ft_free_dvector(beta, 1, sma); beta = 0;
  40.         sma = ma;
  41.         smfit = mfit;
  42.         if ((oneda=Ft_dmatrix(1,mfit,1,1)) == NULL) {
  43.             fprintf(stderr, "mrqmin: Allocation error.\n");
  44.             Ft_catcher(ERRR);
  45.         }
  46.         if ((aold=Ft_dvector(1,ma)) == NULL) {
  47.             fprintf(stderr, "mrqmin: Allocation error.\n");
  48.             Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  49.             Ft_catcher(ERRR);
  50.         }
  51.         if ((da=Ft_dvector(1,ma)) == NULL) {
  52.             Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  53.             Ft_free_dvector(aold, 1, ma); aold = 0;
  54.             fprintf(stderr, "mrqmin: Allocation error.\n");
  55.             Ft_catcher(ERRR);
  56.         }
  57.         if ((beta=Ft_dvector(1,ma)) == NULL) {
  58.             Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  59.             Ft_free_dvector(aold, 1, ma); aold = 0;
  60.             Ft_free_dvector(da, 1, ma); da = 0;
  61.             fprintf(stderr, "mrqmin: Allocation error.\n");
  62.             Ft_catcher(ERRR);
  63.         }
  64.         kk=mfit+1;
  65.         for (j=1;j<=ma;j++) {
  66.             ihit=0;
  67.             for (k=1;k<=mfit;k++)
  68.                 if (lista[k] == j) ihit++;
  69.             if (ihit == 0)
  70.                 lista[kk++]=j;
  71.             else if (ihit > 1) {
  72.                 fprintf(stderr, "mrqmin-1: Bad Lista permutation.\n");
  73.                 Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  74.                 Ft_free_dvector(aold, 1, ma); aold = 0;
  75.                 Ft_free_dvector(da, 1, ma); da = 0;
  76.                 Ft_free_dvector(beta, 1, ma); beta = 0;
  77.                 Ft_catcher(ERRR);
  78.             }
  79.         }
  80.         if (kk != ma+1) {
  81.             fprintf(stderr, "mrqmin-2: Bad Lista permutation.\n");
  82.             Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  83.             Ft_free_dvector(aold, 1, ma); aold = 0;
  84.             Ft_free_dvector(da, 1, ma); da = 0;
  85.             Ft_free_dvector(beta, 1, ma); beta = 0;
  86.             Ft_catcher(ERRR);
  87.         }
  88.         *alamda=0.001;
  89.         if (mrqcof(coef,f,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq)
  90.         == ERRR) { /* first shut */
  91.             Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  92.             Ft_free_dvector(aold, 1, ma); aold = 0;
  93.             Ft_free_dvector(da, 1, ma); da = 0;
  94.             Ft_free_dvector(beta, 1, ma); beta = 0;
  95.             Ft_catcher(ERRR);
  96.         }   
  97.         ochisq=(*chisq);
  98.     }
  99.     for (j=1;j<=mfit;j++) {
  100.         for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
  101.         covar[j][j]=alpha[j][j]*(1.0+(*alamda));
  102.         oneda[j][1]=beta[j];
  103.     }
  104.     if (gaussj(covar,mfit,oneda,1) == ERRR) {
  105.         Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  106.         Ft_free_dvector(aold, 1, ma); aold = 0;
  107.         Ft_free_dvector(da, 1, ma); da = 0;
  108.         Ft_free_dvector(beta, 1, ma); beta = 0;
  109.         Ft_catcher(ERRR);
  110.     }   
  111.     for (j=1;j<=mfit;j++)
  112.         da[j]=oneda[j][1];
  113.     if (*alamda == 0.0) {  /* get error and clean up */
  114.         covsrt(covar,ma,lista,mfit);
  115.         Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  116.         Ft_free_dvector(aold, 1, ma); aold = 0;
  117.         Ft_free_dvector(da, 1, ma); da = 0;
  118.         Ft_free_dvector(beta, 1, ma); beta = 0;
  119.         return(0);
  120.     }
  121.     for (j=1;j<=ma;j++) aold[j]=a[j];
  122.     for (j=1;j<=mfit;j++)
  123.         a[lista[j]] += da[j];
  124.     if (mrqcof(coef,f,y,sig,ndata,a,ma,lista,mfit,covar,da,chisq)
  125.     == ERRR) {
  126.         Ft_free_dmatrix(oneda, 1, mfit, 1, 1); oneda = 0;
  127.         Ft_free_dvector(aold, 1, ma); aold = 0;
  128.         Ft_free_dvector(da, 1, ma); da = 0;
  129.         Ft_free_dvector(beta, 1, ma); beta = 0;
  130.         Ft_catcher(ERRR);
  131.     }   
  132.     if (*chisq < ochisq) {
  133.         *alamda *= 0.1;
  134.         ochisq=(*chisq);
  135.         for (j=1;j<=mfit;j++) {
  136.             for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
  137.             beta[j]=da[j];
  138.         }
  139.     } else {
  140.         for (j=1;j<=ma;j++) a[j]=aold[j];
  141.         *alamda *= 10.0;
  142.         *chisq=ochisq;
  143.     }
  144.     return(0);
  145. }
  146.  
  147. static int mrqcof(double **coef, double *f, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **alpha, double *beta, double *chisq)
  148. {
  149.     int i, j, k;
  150.     double wt, sig2i, dy;
  151.     void Ft_fml_fit(int ndata);
  152.  
  153.     for (j=1;j<=mfit;j++) {
  154.         for (k=1;k<=j;k++)
  155.             alpha[j][k]=0.0;
  156.         beta[j]=0.0;
  157.     }
  158.     *chisq=0.0;
  159.     Ft_fml_fit(ndata);
  160.     for (i=1;i<=ndata;i++) {
  161.         sig2i=1.0/(sig[i]*sig[i]);
  162.         dy=y[i]-f[i];
  163.         for (j=1;j<=mfit;j++) {
  164.             wt=coef[lista[j]][i]*sig2i;
  165.             for (k=1;k<=j;k++)
  166.                 alpha[j][k] += wt*coef[lista[k]][i];
  167.             beta[j] += dy*wt;
  168.         }
  169.         (*chisq) += dy*dy*sig2i;
  170.     }
  171.     for (j=2;j<=mfit;j++)
  172.         for (k=1;k<=j-1;k++)
  173.             alpha[k][j]=alpha[j][k];
  174.     return(0);
  175. }
  176.  
  177. static int covsrt(double **covar, int ma, int *lista, int mfit)
  178. {
  179.     int i,j;
  180.     double swap;
  181.  
  182.     for (j=1;j<ma;j++)
  183.         for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
  184.     for (i=1;i<mfit;i++)
  185.         for (j=i+1;j<=mfit;j++) {
  186.             if (lista[j] > lista[i])
  187.                 covar[lista[j]][lista[i]]=covar[i][j];
  188.             else
  189.                 covar[lista[i]][lista[j]]=covar[i][j];
  190.         }
  191.     swap=covar[1][1];
  192.     for (j=1;j<=ma;j++) {
  193.         covar[1][j]=covar[j][j];
  194.         covar[j][j]=0.0;
  195.     }
  196.     covar[lista[1]][lista[1]]=swap;
  197.     for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
  198.     for (j=2;j<=ma;j++)
  199.         for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
  200.  
  201.     return(0);
  202. }
  203.  
  204. #define SWAP(a,b) {double temp=(a);(a)=(b);(b)=temp;}
  205.  
  206. static int gaussj(double **a, int n, double **b, int m)
  207. {
  208.     int *indxc,*indxr,*ipiv;
  209.     int i,icol,irow,j,k,l,ll;
  210.     double big,dum,pivinv;
  211.  
  212.     AIVECTOR(indxc, 1, n, "gaussj", return);
  213.     AIVECTOR(indxr, 1, n, "gaussj", return);
  214.     AIVECTOR(ipiv, 1, n, "gaussj", return);
  215.  
  216.     icol = irow = 1;
  217.     for (j=1;j<=n;j++)
  218.         ipiv[j]=0;
  219.     for (i=1;i<=n;i++) {
  220.         big=0.0;
  221.         for (j=1;j<=n;j++)
  222.             if (ipiv[j] != 1)
  223.                 for (k=1;k<=n;k++) {
  224.                     if (ipiv[k] == 0) {
  225.                         if (fabs(a[j][k]) >= big) {
  226.                             big=fabs(a[j][k]);
  227.                             irow=j;
  228.                             icol=k;
  229.                         }
  230.                     } else if (ipiv[k] > 1) {
  231.                         fprintf(stderr, "gaussj: Singular Matrix 1.\n");
  232.                         return(ERRR);
  233.                     }
  234.                 }
  235.         ++(ipiv[icol]);
  236.         if (irow != icol) {
  237.             for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
  238.             for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
  239.         }
  240.         indxr[i]=irow;
  241.         indxc[i]=icol;
  242.         if (a[icol][icol] == 0.0) {
  243.             fprintf(stderr, "gaussj: Singular Matrix 2.\n");
  244.             return(ERRR);
  245.         }
  246.         pivinv=1.0/a[icol][icol];
  247.         a[icol][icol]=1.0;
  248.         for (l=1;l<=n;l++) a[icol][l] *= pivinv;
  249.         for (l=1;l<=m;l++) b[icol][l] *= pivinv;
  250.         for (ll=1;ll<=n;ll++)
  251.             if (ll != icol) {
  252.                 dum=a[ll][icol];
  253.                 a[ll][icol]=0.0;
  254.                 for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
  255.                 for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
  256.             }
  257.     }
  258.     for (l=n;l>=1;l--) {
  259.         if (indxr[l] != indxc[l])
  260.             for (k=1;k<=n;k++)
  261.                 SWAP(a[k][indxr[l]],a[k][indxc[l]]);
  262.     }
  263.     return(0);
  264. }
  265.  
  266. #undef SWAP
  267.  
  268. int Ft_lfit(double **coef, double *y, double *sig, int ndata, double *a, int ma, int *lista, int mfit, double **covar, double *chisq)
  269. {
  270.     int k,kk,j,ihit,i;
  271.     double ym,wt,sum,sig2i,**beta;
  272.     double tmp;
  273.  
  274.     ADMATRIX(beta, 1, ma, 1, 1, "lfit", Ft_catcher);
  275.     kk=mfit+1;
  276.     for (j=1;j<=ma;j++) {
  277.         ihit=0;
  278.         for (k=1;k<=mfit;k++)
  279.             if (lista[k] == j) ihit++;
  280.         if (ihit == 0)
  281.             lista[kk++]=j;
  282.         else if (ihit > 1) {
  283.             fprintf(stderr, "lfit: Bad 'adjust' permutation-1.");
  284.             Ft_catcher(ERRR);
  285.         }
  286.     }
  287.     if (kk != (ma+1)) {
  288.         fprintf(stderr, "lfit: Bad 'adjust' permutation-2.");
  289.         Ft_catcher(ERRR);
  290.     }
  291.     for (j=1;j<=mfit;j++) {
  292.         for (k=1;k<=mfit;k++) covar[j][k]=0.0;
  293.         beta[j][1]=0.0;
  294.     }
  295.     for (i=1;i<=ndata;i++) {
  296.         ym=y[i];
  297.         if (mfit < ma)
  298.             for (j=(mfit+1);j<=ma;j++)
  299.                 ym -= a[lista[j]]*coef[lista[j]][i];
  300.         sig2i=1.0/(sig[i]*sig[i]);
  301.         for (j=1;j<=mfit;j++) {
  302.             wt=coef[lista[j]][i]*sig2i;
  303.             for (k=1;k<=j;k++)
  304.                 covar[j][k] += wt*coef[lista[k]][i];
  305.             beta[j][1] += ym*wt;
  306.         }
  307.     }
  308.     if (mfit > 1)
  309.         for (j=2;j<=mfit;j++)
  310.             for (k=1;k<=j-1;k++)
  311.                 covar[k][j]=covar[j][k];
  312.     if (gaussj(covar,mfit,beta,1) == ERRR) {
  313.         Ft_catcher(ERRR);
  314.     }
  315.     for (j=1;j<=mfit;j++) a[lista[j]]=beta[j][1];
  316.     *chisq=0.0;
  317.     for (i=1;i<=ndata;i++) {
  318.         for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*coef[j][i];
  319.         tmp = (y[i]-sum)/sig[i];
  320.         *chisq += tmp*tmp;
  321.     }
  322.     covsrt(covar,ma,lista,mfit);
  323.     return(0);
  324. }
  325.  
  326.